//Q:void ViewPointServer::goalPubTriggerCallback功能是什么
// 从launch文件中读取获取文件路径 todo
// 从launch文件中读取获取odom_topic/ goal topic todo
#include "../include/view_point_server/view_point_server.h"

ViewPointServer::ViewPointServer(SERVER_MODE mode, ros::NodeHandle &nh)
{
    nh_                 = nh;
    cur_waypoint_index_ = 0;
    save_file_path_     = ros::package::getPath("view_point_server") + "/data/waypoints.txt";
    std::cout << "[Waypoint Server] | save file path: " << save_file_path_ <<std::endl;

    std::string mode_str, odom_topic, goal_topic;
    readParam<std::string>(nh_, "vp_server/mode", mode_str, "sub");
    readParam<std::string>(nh_, "vp_server/odom_topic", odom_topic, "/drone_0_visual_slam/odom");
    readParam<std::string>(nh_, "vp_server/goal_pub_topic", goal_topic, "/goal_with_id_from_station");
    readParam<double>(nh_, "vp_server/vis_spheral_size", vis_spheral_size_, 0.1);
    readParam<double>(nh_, "vp_server/nxt_goal_distance_threshold", _nxt_goal_distance_threshold, 0.3);

    target_vis = nh_.advertise<visualization_msgs::MarkerArray>("/preset_target_sever/vis_info", 10);

    if (mode_str == "sub")
        _mode = RECORD;
    else if (mode_str == "pub")
        _mode = PUBLISH;

    if(_mode == RECORD)       //RECORD模式
    {
        odom_sub_             = nh_.subscribe("/drone_0_visual_slam/odom", 10, &ViewPointServer::odomCallback, this);
        record_trigger_sub_   = nh_.subscribe("/move_base_simple/goal", 10, &ViewPointServer::recordTriggerCallback, this);
    }
    else if(_mode == PUBLISH)  //PUBLISH 模式
    {
        odom_sub_             = nh_.subscribe("/drone_0_visual_slam/odom", 10, &ViewPointServer::odomCallback, this);
        goal_pub_             = nh_.advertise<quadrotor_msgs::GoalSet>("/goal_with_id_from_station", 10);
        goal_trigger_sub_     = nh_.subscribe("/initialpose", 10, &ViewPointServer::goalPubTriggerCallback, this);

        if(!loadWaypoints(save_file_path_)){
            ROS_FATAL("Failed to load waypoints from file");
        }
        already_published_ = false;
    }
    else
    {
        ROS_ERROR_STREAM("Invalid mode");
    }
}

ViewPointServer::~ViewPointServer()
{
    // 保存航点
    if (_mode == RECORD && !waypoints_.empty())
        saveWaypoints(save_file_path_);
}

void ViewPointServer::odomCallback(const nav_msgs::OdometryConstPtr &msg)
{
    current_odom_ = *msg;
}

void ViewPointServer::recordTriggerCallback(const geometry_msgs::PoseStampedConstPtr &msg) {
    // 获取航点位置
    geometry_msgs::Point waypoint;
    waypoint.x = current_odom_.pose.pose.position.x;
    waypoint.y = current_odom_.pose.pose.position.y;
    waypoint.z = current_odom_.pose.pose.position.z;

    // 添加到航点列表
    waypoints_.push_back(waypoint);

    // 输出航点信息
    ROS_INFO_STREAM("View Point added: (" << waypoint.x << ", " << waypoint.y << ", " << waypoint.z << ")");
    allGoalVisualization();
}

void ViewPointServer::goalPubTriggerCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) {
    static bool first_trigger = true;
    if (first_trigger){
        ROS_WARN_STREAM("First trigger, please check goal correctly & trigger again!");
        first_trigger = false;
        allGoalVisualization();
        goalVisualization(cur_waypoint_index_);
    }else{
        already_published_ = false;
        goal_pub_timer_    = nh_.createTimer(ros::Duration(0.03), &ViewPointServer::goalHandler, this);//处理发布信息
        ROS_WARN_STREAM(" \n\n ------------------- \nPublish Timer Start!");
    }
}

void ViewPointServer::saveWaypoints(const std::string &save_file_path)
{
    // 打开文件保存航点
    std::ofstream file(save_file_path);
    if (file.is_open())
    {
        for (const auto &waypoint : waypoints_)
        {
            file << waypoint.x << " " << waypoint.y << " " << waypoint.z << std::endl;
        }
        file.close();
        ROS_INFO_STREAM("View Points saved to " << save_file_path);
    }
    else
    {
        ROS_ERROR_STREAM("Failed to open file: " << save_file_path);
    }
}

bool ViewPointServer::loadWaypoints(const std::string &save_file_path) {
    // 打开文件读取航点
    std::ifstream file(save_file_path);
    std::vector<geometry_msgs::Point> load_waypoints;
    if (file.is_open()) {
        int cur_load_waypoint_index_ = 0;
        std::string line;
        while (std::getline(file, line)) {
            // 使用stringstream来解析每一行
            std::stringstream ss(line);
            float x, y, z;
            if (ss >> x >> y >> z) {
                geometry_msgs::Point load_waypoint;
                load_waypoint.x = x;
                load_waypoint.y = y;
                load_waypoint.z = z;
                load_waypoints.push_back(load_waypoint);
                cur_load_waypoint_index_++;
                std::cout <<"--------------------------------\n" <<cur_load_waypoint_index_<< std::endl;
                std::cout << load_waypoint << std::endl;
            } else {
                ROS_ERROR_STREAM("Invalid format in file at line " << cur_load_waypoint_index_ + 1 << ": " << line);
                file.close();
                return false;
            }
        }
        file.close();
    } else {
        ROS_ERROR_STREAM("Failed to open file: " << save_file_path);
        return false;
    }

    if (waypoints_.empty()){
        waypoints_ = load_waypoints;
        allGoalVisualization();
        goalVisualization(cur_waypoint_index_);
        std::cout<<"WayPoints size : "<<waypoints_.size()<<std::endl;
        return true;
    }
    return false;
}

void ViewPointServer::goalHandler(const ros::TimerEvent &event) {
    if (waypoints_.empty()) return;
    if (waypoints_.size() <= cur_waypoint_index_) {
        ROS_INFO_THROTTLE(2, "Waypoint navigation finished..");
        ROS_FATAL("No more waypoints to navigate, auto QUIT!!!!");
        return ;
    }

    geometry_msgs::Point current_goal  = waypoints_.at(cur_waypoint_index_);

    if (!already_published_)
    {
        quadrotor_msgs::GoalSet goal_msg;
        goal_msg.to_drone_ids.push_back(0);
        goal_msg.goal.push_back(current_goal);
        goal_msg.yaw.push_back(0.0);
        goal_msg.look_forward = true;
        goal_msg.goal_to_follower = false;
        goal_pub_.publish(goal_msg);

        already_published_ = true;
        ROS_INFO_STREAM("Published goal <" << cur_waypoint_index_ << "> : (" << current_goal.x << ", " << current_goal.y << ", " << current_goal.z << ")");
        goalVisualization(cur_waypoint_index_);
        allGoalVisualization();
    }

    // 判断当前位置和当前目标点的距离
    Eigen::Vector3d cur_pos(current_odom_.pose.pose.position.x, current_odom_.pose.pose.position.y, current_odom_.pose.pose.position.z);
    Eigen::Vector3d cur_goal(current_goal.x, current_goal.y, current_goal.z);
    double distance = (cur_pos - cur_goal).norm();
    if (distance < _nxt_goal_distance_threshold)
    {
        cur_waypoint_index_ ++;
        already_published_ = false;
    }
}

void ViewPointServer::goalVisualization(const int& target_id){
    visualization_msgs::MarkerArray marker_array;
    visualization_msgs::Marker sphere;
    sphere.header.frame_id = "world";
    sphere.header.stamp = ros::Time::now();
    sphere.ns = "current_waypoint";
    sphere.id = 0;
    sphere.type = visualization_msgs::Marker::SPHERE;
    sphere.action = visualization_msgs::Marker::ADD;
    sphere.pose.position.x = waypoints_[target_id].x;
    sphere.pose.position.y = waypoints_[target_id].y;
    sphere.pose.position.z = waypoints_[target_id].z;
    sphere.pose.orientation.x = 0.0;
    sphere.pose.orientation.y = 0.0;
    sphere.pose.orientation.z = 0.0;
    sphere.pose.orientation.w = 1.0;
    sphere.scale.x = vis_spheral_size_;
    sphere.scale.y = vis_spheral_size_;
    sphere.scale.z = vis_spheral_size_;
    sphere.color.r = 0.0; // 红色分量
    sphere.color.g = 1.0; // 绿色分量
    sphere.color.b = 0.0; // 蓝色分量
    sphere.color.a = 1.0; // Alpha（透明度）

    marker_array.markers.push_back(sphere);
    target_vis.publish(marker_array);
}

void ViewPointServer::allGoalVisualization(){
    visualization_msgs::MarkerArray marker_array;
    visualization_msgs::Marker sphere_list;
    sphere_list.header.frame_id = "world";
    sphere_list.header.stamp = ros::Time::now();
    sphere_list.ns = "all_waypoints";
    sphere_list.id = 0;
    sphere_list.type = visualization_msgs::Marker::SPHERE_LIST;
    sphere_list.action = visualization_msgs::Marker::ADD;
    sphere_list.pose.orientation.x = 0.0;
    sphere_list.pose.orientation.y = 0.0;
    sphere_list.pose.orientation.z = 0.0;
    sphere_list.pose.orientation.w = 1.0;
    sphere_list.scale.x = vis_spheral_size_;
    sphere_list.scale.y = vis_spheral_size_;
    sphere_list.scale.z = vis_spheral_size_;
    sphere_list.color.r = 1.0; // 红色分量
    sphere_list.color.g = 0.0; // 绿色分量
    sphere_list.color.b = 0.0; // 蓝色分量
    sphere_list.color.a = 0.8; // Alpha（透明度）

    for(auto & waypoint : waypoints_){
        geometry_msgs::Point point;
        point.x = waypoint.x;
        point.y = waypoint.y;
        point.z = waypoint.z;
        sphere_list.points.push_back(point);
    }
    marker_array.markers.push_back(sphere_list);

    for (int i = 0; i < waypoints_.size(); i++){
        visualization_msgs::Marker text;
        text.header.frame_id = "world";
        text.header.stamp = ros::Time::now();
        text.ns = "all_waypoints_text";
        text.id = i;
        text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
        text.action = visualization_msgs::Marker::ADD;
        text.pose.position.x = waypoints_[i].x + 0.2;
        text.pose.position.y = waypoints_[i].y + 0.2;
        text.pose.position.z = waypoints_[i].z + 0.2;
        text.pose.orientation.w = 1.0;
        text.scale.z = 0.5;
        text.color.r = 1.0; // 红色分量
        text.color.g = 0.0; // 绿色分量
        text.color.b = 0.0; // 蓝色分量
        text.color.a = 1.0; // Alpha（透明度）
        text.text = std::to_string(i + 1);
        marker_array.markers.push_back(text);
    }
    target_vis.publish(marker_array);
}



template<typename T>
void ViewPointServer::readParam(ros::NodeHandle &node, std::string param_name, T &param_val, T default_val) {
    if (!node.param(param_name, param_val, default_val))
        ROS_WARN_STREAM("[RC-FSM]: parameter " << param_name << " not found, using default value: " << default_val);
    else
        ROS_INFO_STREAM("[RC-FSM]: parameter " << param_name << " found: " << param_val);
}
